package butines.core.game.pathfinder;

import java.util.Collections;
import java.util.LinkedList;
import java.util.List;

import butines.core.game.graph.Edge;
import butines.core.game.graph.Graph;
import butines.core.game.graph.Node;

public class PathFinderAStar implements PathFinder {

	private Graph<NavNode> graph;
	private Heuristic heuristic;
	private NavNode source;
	private NavNode target;
	private boolean found;
	private List<NavNode> path;

	private LinkedList<NodeWrapper> openList;
	
	private NodeWrapper[] wrappers;

	private NodeWrapper current;

	public PathFinderAStar(Graph<NavNode> graph, Heuristic heuristic) {
		this.graph = graph;
		this.heuristic = heuristic;
		this.wrappers = new NodeWrapper[graph.getNumNodes()];
		for (int i = 0; i < wrappers.length; i++) {
			wrappers[i] = new NodeWrapper(i);
		}
	}

	public void search(int iSource, int iTarget) {
		openList = new LinkedList<NodeWrapper>();
		path = new LinkedList<NavNode>();
		found = false;
		
		source = graph.getNode(iSource);
		target = graph.getNode(iTarget);

		for (int i = 0; i < wrappers.length; i++) {
			wrappers[i].init();
		}
		
		current = wrappers[iSource];
		current.gCost = 0;
		current.fCost = 0;
		current.close = true;
		current.parent = current;
		
		openList.add(current);
	}

	private void createPath() {
		NavNode node = target;
		path.add(node);
		while (node != source) {
			NodeWrapper parent = wrappers[node.getId()].parent;
			node = parent.node;
			path.add(node);
		}
	}

	public void next() {
		current = openList.removeFirst();
		current.close = true;

		if (current.node == target) {
			createPath();
			found = true;
			return;
		}

		for (Edge<NavNode> edge : current.node) {
			int to = edge.getTo().getId();
			if (wrappers[to].close) {
				continue;
			}

			double hCost = heuristic.calcule(wrappers[to].node, target);
			double gCost = current.gCost + edge.getCost();
			if (wrappers[to].parent == null) {
				wrappers[to].fCost = gCost + hCost;
				wrappers[to].gCost = gCost;
				wrappers[to].hCost = hCost;
				wrappers[to].parent = current;
				openList.add(wrappers[to]);
			} else if (wrappers[to].gCost > gCost) {
				wrappers[to].fCost = gCost + hCost;
				wrappers[to].gCost = gCost;
				wrappers[to].parent = current;
			}

		}
		Collections.sort(openList);
	}

	public boolean hasNext() {
		return !openList.isEmpty() && !found;
	}

	public boolean isFound() {
		return found;
	}

	public List<NavNode> getPath() {
		return path;
	}
	
	public NavNode getSource() {
		return source;
	}

	public NavNode getTarget() {
		return target;
	}
	
	public Node<Edge<NavNode>> getCurrent() {
		return current == null ? null : current.node;
	}


	class NodeWrapper implements Comparable<NodeWrapper> {

		int id;
		NavNode node;
		NodeWrapper parent;
		double gCost;
		double fCost;
		double hCost;
		boolean close;

		public NodeWrapper(int id) {
			this.id = id;
		}

		public void init() {
			gCost = 0;
			gCost = 0;
			hCost = 0;
			close = false;
			node = graph.getNode(id);
		}

		public int compareTo(NodeWrapper other) {
			double value = this.fCost - other.fCost;
			if (value > 0) {
				return 1;
			}
			if (value < 0) {
				return -1;
			}
			return 0;
		}
		
	}

}
